/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* Matches the heading between two rangescans 
**/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <vector>
#include "EnvironmentMeasurement2D.h" ///< Input describtion

struct TAngleMatch{
		float angle;
		float corrVal;
		
		TAngleMatch(){
				angle = 0; corrVal = 0;
		}
		TAngleMatch(float a, float c){
				angle = a; corrVal=c;
		}
		void set(float a, float c){
				angle = a; corrVal=c;
		}
		
		bool operator<(const TAngleMatch &s) {
				if(corrVal<s.corrVal) return true; 
				return false;
		}
		bool operator<=(const TAngleMatch &s) {
				if(corrVal<=s.corrVal) return true; 
				return false;
		}
		bool operator>(const TAngleMatch &s) {
				if(corrVal>s.corrVal) return true; 
				return false;
		}
		bool operator>=(const TAngleMatch &s) {
				if(corrVal>=s.corrVal) return true; 
				return false;
		}
		
};

class CHoughAngleMatcher{
		public:
				/**
				* Constructor
				* @param numberofrangevalues Fixed Number, which is used in discretation
				* @param numberofanglevalues Fixed Number, which is used in discretation 
				*/
				CHoughAngleMatcher(int numberofrangevalues, int numberofanglevalues);
				~CHoughAngleMatcher();
				
				/**
				* Finds a set of candidate angles between two given datasets
				* The method assumes that the data points are taken approximately from
				* the same environment, but from different angles. The mathing
				* is based on correlating the HoughSpectrum of both sets.
				* The result is returned as a (sorted) vector of angle, correlation pairs
				* The larger the correlation the better.
				* @param &ref reference dataset
				* @param &meas measurement datasets
				* @param MaxR Maximum range (from origin) that the data point can lie in
				* @param &matches the result
				*/
				void match(CEnvironmentMeasurement2D &ref, 
									 CEnvironmentMeasurement2D &meas, 
									 float MaxR,
									 std::vector<TAngleMatch> &matches);
				
		private:
				int numRangeValues;  ///Number of discretized range values
				int numThetaValues;  ///Number of discretized angle values
				float   *sinlut;
				float   *coslut;
				float *houghTf1;      ///Hought transform for the reference
				float *houghTf2;      ///Hought transform for the Measurement
				float *houghSpec1;    ///Hough Spectrum for the reference 
				float *houghSpec2;    ///Hough Spectrum for the Measurement
				float *corr;          ///Correlation values
				/**
				 * Make a Hough Transform 
				 * @param *hth The hough transform
				 * @param &meas EnvironmentMeasurement2D object containing the (x,y) points
				 * @param MaxR   Maximum range (from origin to furthest point)
				 **/
				void houghtransform(float *hth,CEnvironmentMeasurement2D &meas, float MaxR);
				/**
				* Compute Hough Spectrum
				* @param *hth pointer to Hough Transformation
				* @param *spectrum pointer to spectrum (output)
				*/
				void computeHoughSpectrum(float *hth, float *spectrum);
				
				/**
				* Calculates the correlation between Hough Spectrums
				* Uses the class variables @houghSpec1 and @houghSpec2 and
				* stores the result into class variable @corr
				* Assumes that the spectrums and histograms are already computed
				*/
				void computeCorrelation();
				/**
				* Finds all local maximas from the correlation function
				* Uses the class variable  @corr and assumes it is already calculated
				* @param &out vector of indices that corresponds to local maximas
				*/
				void findMaximumCorrelations(std::vector<int> &out);
				
				/**
				* Sorts the matches (smallest value first)
				**/
				void sort(std::vector<TAngleMatch> &matches);
				
};
